clc
clear

% Physical constants
Rs  = 7.9e-3;
Ld  = .23e-3;
Lq  = .56e-3;
phi = 104e-3;
rpm = 6000;
wm  = rpm/60*2*pi;
np  = 4;
wr  = wm*np/2;

% Current controller
id_ref = 160;
iq_ref = 0;
t_rc   = 2e-3;
a = log(9)/t_rc;
kp_d = a*Ld;
kp_q = a*Lq;
ki_d = a*a*Ld;
ki_q = a*a*Lq;
Rad  = a*Ld-Rs;
Raq  = a*Lq-Rs;

% Simulation
ref_step_time = 1;
sim('model_task1',4,simset('Solver','ode45','MaxStep',1e-5));

% Find 90%
t = pmsm.time;
i = pmsm.signals.values;
F = controller.signals.values(:,1:4);
Ref = controller.signals.values(:,5:6);
N = length(t);
t_rise_d = t(find(id_ref*0.9<i(:,1),1))-t(find(id_ref*0.1<i(:,1),1));
t_rise_q = t(find(iq_ref*0.9<i(:,2),1))-t(find(iq_ref*0.1<i(:,2),1));
disp(['Rise time d: ' num2str(1000*t_rise_d) ' ms'])
disp(['Rise time q: ' num2str(1000*t_rise_q) ' ms'])

% Plotting
subplot(2,1,1)
plot(t,i);hold on
plot(t,Ref,'--');hold off
legend('i_d','i_q')
subplot(2,1,2)
plot(t,F)
legend('D_p','D_i','Q_p','Q_i')
